#include "BorunteRobot.hpp"

using namespace bl;
BorunteRobot::BorunteRobot()
    : Node("BLSmartRobot")
    {
      publisher_ = this->create_publisher<std_msgs::msg::String>("BorunteRobotState", 10);
      subscription_ = this->create_subscription<std_msgs::msg::String>(
      	"BorunteRobot", 10, std::bind(&BorunteRobot::run, this, _1));
      jointStatePublisher_ = motion_planning_api_tutorial_node->create_publisher<sensor_msgs::msg::JointState>("/joint_states", 10);
      
      
      node_options.automatically_declare_parameters_from_overrides(true);
      
      robot_model_loader::RobotModelLoader robot_model_loader(this, "robot_description");
  	robot_model = robot_model_loader.getModel();
  	
  	robot_state = new moveit::core::RobotState(robot_model);
   	joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
      planning_scene = new planning_scene::PlanningScene(robot_model);
	planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
	
	printJointInfo();
    }

void BorunteRobot::run()
    {
      auto message = std_msgs::msg::String();
      message.data = "BL:" ;
      RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
      publisher_->publish(message);
    }
    
};
void printJointInfo(){
	std::vector<std::string>& joint_names = joint_model_group->getJointModelNames() ;
	std::vector<double> joint_values;
	
	RCLCPP_INFO(this->get_logger(), "Current Joint State:");
	planning_scene->getCurrentState().printStatePositions();
	planning_scene->getCurrentState().copyJointGroupPositions( joint_model_group, joint_values);
  
	  for (std::size_t i = 0; i < joint_names.size(); ++i)
	  {
	    RCLCPP_INFO(this->get_logger(), "Joint %s: %d", joint_names[i].c_str(), (int)(joint_values[i]*180/3.1416) );
	  }
}

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<BorunteRobot>());
  rclcpp::shutdown();
  return 0;
}
